
#include "sim.hpp"
#include "viewer.hpp"
#include "path.hpp"

// 纯定位形式
int main(){
    std::vector<Eigen::Vector3d> Walls;// 定义墙面集合
    // 二维度状态下墙壁是两条直线参数
    Walls.push_back(Eigen::Vector3d(0., 1., 0.));//---墙
    Walls.push_back(Eigen::Vector3d(1., 0., 0.));// |
    // 设置机器人类
    robots * robot = new threeWheelRobots();
    dynamic_cast<threeWheelRobots*>(robot)->setLaserWalls(Walls);
    // 显示的部分
    viewer * show = new viewer(robot);
    show->setWalls(Walls);// 设定显示部分的墙参0            
    // 进行显示线程
    std::thread showthread(std::ref(*show));
    showthread.detach();
    // 路经仿真部分
    path * pather = new path1();

    Simer simer(robot, show, pather);
    // 仿真一段小轨迹
    std::vector<Eigen::Vector3d> inWall;
    inWall.push_back(Eigen::Vector3d(0., 1., 0.));//---墙 0激光
    inWall.push_back(Eigen::Vector3d(0., 1., 0.));//---墙 1激光
    inWall.push_back(Eigen::Vector3d(1., 0., 0.));// |
    inWall.push_back(Eigen::Vector3d(1., 0., 0.));// |
    simer.getRobotPosAndTransAndExt(inWall);// 同时也对外参也进行优化
    
    while(1);

    return 0;
}

